#ifndef __TOOLFUNCTION_H
#define __TOOLFUNCTION_H



typedef struct
{
	float X_Ray;
	float Y_Ray;
	float X_Val[5];		//Value
	float Y_Val[5];
	float X_Ave;		//Average Value
	float Y_Ave;
	
}Point_InitDefStruct;

typedef struct
{
	float P;            //取1
	float P_;  			//公式中的p'
	float X;            //取0
	float X_; 			//公式中的X'
	float K;            //取0
	float Q;			//取0.01 噪声
	//R大，更相信预测，那么传感器反应就会迟钝，反之亦然
	float R;			//取0.2 这里0.5算有点点大
	
}Kalman_1_DefStruct;

// Kalman_1_DefStruct Kal=
// {.P = 1, .Q = 0.01, .R = 0.2};

typedef struct
{
	float Q_angle;		//取0.001 角度数据置信度，角度噪声的协方差
	float Q_gyro;		//取0.003 角速度数据置信度，角速度噪声的协方差  
	float R_angle;		//取0.5 加速度计测量噪声的协方差
	float dt;			//取0.01 采样周期即计算任务周期10ms
	float Q_bias;		//Q_bias:陀螺仪的偏差
	float K_0, K_1;		//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差
	float PP[2][2];		//取{ { 1, 0 },{ 0, 1 } } 过程协方差矩阵P，初始值为单位阵
	
}Kalman_2_DefStruct;

// Kalman_2_DefStruct Kal=
// {.Q_angle = 0.001, .Q_gyro = 0.003, .R_angle = 0.5,
//  .dt = 0.01, .PP[2][2] = {1,0,0,1}};


typedef enum {
    FLITER_NEGATIVE = 0,
    FLITER_POSITIVE = 1
} FLITER_Mode;


void  Tool_Filter_3_5(Point_InitDefStruct *PointProto);
float Tool_Filter_MPU6050(FLITER_Mode mode, float Yaw_Value);
float Tool_Filter_Kalman_1(float Filter_Value, Kalman_1_DefStruct *kalman);
float Tool_Filter_Kalman_2(float acc,float gyro,Kalman_2_DefStruct *kalman);

int Tool_POW(int m,int n);
int Tool_D2B(int n);

//void Tool_Delay_us(uint32_t us);

#endif


